/**
 ****************************************************************************************************
 * @file        foc_smo.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       滑模代码
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */

#include "foc_smo.h"
#include "d_math.h"
#include <stdio.h>

#define SMO_Kslide 1.868f    // 滑模增益K
#define SMO_Kslf   0.12566f  // 低通滤波系数

#define LPF_SMO_A 0.23905722361f  // 低通滤波系数，截止频率1000Hz
#define LPF_SMO_B 0.76094277639f

// 饱和函数
static float Sat(float s, float delta)
{
    if(s > delta)
        return 1;
    else if(s < -delta)
        return -1;
    else
        return s / delta;
}

void FOC_SMO_Init(FOC_SMO_t* foc_smo, float Rs, float Ls, float Ts, uint32_t pole_pairs, float max_speed)
{
    foc_smo->Rs = Rs;
    foc_smo->Ls = Ls;
    foc_smo->Rs_Ls = Rs / Ls;
    foc_smo->_1_Ls = 1.0f / Ls;
    foc_smo->Ts = Ts;

    foc_smo->Encoder.pole_pairs = pole_pairs;
    foc_smo->Encoder.Max_MecSpeed = max_speed;

    PLL_Init(&foc_smo->pll, Ts);
}

/* 滑模角度计算 */
void FOC_SMO_Angle_Calc(FOC_SMO_t* foc_smo, I_alphabeta_t Real_I, V_alphabeta_t Real_U)
{
    float Ts = foc_smo->Ts;
    float Rs_Ls = foc_smo->Rs_Ls;
    float _1_Ls = foc_smo->_1_Ls;

    foc_smo->Real_U = Real_U;
    foc_smo->Real_I = Real_I;

    /* 积分计算观测电流 */
    foc_smo->Est_I.i_alpha +=
        Ts * (-Rs_Ls * foc_smo->Est_I.i_alpha + _1_Ls * (foc_smo->Real_U.v_alpha - foc_smo->Est_V.i_alpha));
    foc_smo->Est_I.i_beta +=
        Ts * (-Rs_Ls * foc_smo->Est_I.i_beta + _1_Ls * (foc_smo->Real_U.v_beta - foc_smo->Est_V.i_beta));

    /* 计算误差 */
    foc_smo->Est_I_err.i_alpha = foc_smo->Est_I.i_alpha - foc_smo->Real_I.i_alpha;
    foc_smo->Est_I_err.i_beta = foc_smo->Est_I.i_beta - foc_smo->Real_I.i_beta;

    foc_smo->Est_V.i_alpha = foc_smo->h * Sat(foc_smo->Est_I_err.i_alpha, 0.5f);
    foc_smo->Est_V.i_beta = foc_smo->h * Sat(foc_smo->Est_I_err.i_beta, 0.5f);

    // 低通滤波
    foc_smo->Est_U.i_alpha = foc_smo->Est_U.i_alpha * 0.1f + foc_smo->Est_V.i_alpha * 0.9f;
    foc_smo->Est_U.i_beta = foc_smo->Est_U.i_beta * 0.1f + foc_smo->Est_V.i_beta * 0.9f;

    /* 反正切法求 */
    // foc_smo->atan_theta = Limit_Angle(atan2f(foc_smo->Est_U.alpha, foc_smo->Est_U.beta));
    // // foc_smo->atan_theta = Limit_Angle(arm_sin_f32(foc_smo->Est_U.beta, foc_smo->Est_U.alpha));
    // foc_smo->speed.ElAngle = foc_smo->atan_theta;

    /*锁相环法求*/
    PLL_Cale_SMO(&foc_smo->pll, (float)foc_smo->Encoder.dir * foc_smo->Est_U.i_alpha,
                 (float)foc_smo->Encoder.dir * foc_smo->Est_U.i_beta);
    //    foc_smo->speed.ElAngle = Limit_Angle(foc_smo->pll.Angle);        /*加上补偿后的角度*/
    foc_smo->Encoder.ElAngle = foc_smo->pll.Angle;
}

/* 滑模速度计算 */
void FOC_SMO_Speed_Calc(FOC_SMO_t* foc_smo)
{
    // TODO：以下两句作用未知
    //    float tmp = Move_Filter_calculate(&foc_smo->pll.move_filter);
    //    tmp = tmp / foc_smo->speed.pole_pairs * 60.0f / M_2PI;
    foc_smo->Encoder.MecSpeed = foc_smo->pll.move_filter.val / (float)foc_smo->Encoder.pole_pairs * 60.0f / M_2PI;

    if(foc_smo->Encoder.MecSpeed > foc_smo->Encoder.Max_MecSpeed)
    {
        /*pll计算异常*/
        FOC_SMO_Clear(foc_smo);
        printf("滑模PLL计算异常\r\n");
    }

    // if(fabsf(foc_smo->speed.AvrMecSpeed)>1000){
    //     foc_smo->h = 0.1f;
    // }else{
    //     foc_smo->h = 0.1f;
    // }
}

/*滑模参数清零*/
void FOC_SMO_Clear(FOC_SMO_t* foc_smo)
{
    PLL_Clear(&foc_smo->pll);
}
